// 源文件不需要#pragma once，删除！
#include "lio_ndt/method/pcl_ndt.h"
#include <iostream>
#include <boost/make_shared.hpp>  // 确保boost智能指针可用

namespace lio_ndt {

// 构造函数实现（与头文件声明一致，无参数）
PCLNDT::PCLNDT()
    : target_(boost::make_shared<pcl::PointCloud<PointT>>()),  // 初始化目标点云
      resolution_(1.0),  // 与头文件变量名一致
      max_iter_(30),     // 与头文件变量名一致
      trans_eps_(1e-6),  // 与头文件变量名一致
      last_fitness_(std::numeric_limits<double>::infinity()) {  // 修正初始值（之前是0.0，与逻辑一致）
    // 初始化NDT参数（与成员变量一致）
    ndt_.setResolution(resolution_);
    ndt_.setMaximumIterations(max_iter_);
    ndt_.setTransformationEpsilon(trans_eps_);
}

// SetResolution实现（参数类型double，与头文件一致）
void PCLNDT::SetResolution(double res) {
    resolution_ = res;
    ndt_.setResolution(resolution_);
}

// SetMaximumIterations实现（参数类型int，与头文件一致）
void PCLNDT::SetMaximumIterations(int it) {
    max_iter_ = it;
    ndt_.setMaximumIterations(max_iter_);
}

// SetTransformationEpsilon实现（参数类型double，与头文件一致）
void PCLNDT::SetTransformationEpsilon(double eps) {
    trans_eps_ = eps;
    ndt_.setTransformationEpsilon(trans_eps_);
}

// SetTargetCloud实现（与头文件纯虚函数签名一致，含const&和override）
bool PCLNDT::SetTargetCloud(const CloudPtr& target) {
    if (!target || target->empty()) {
        std::cerr << "[PCLNDT] SetTargetCloud failed: Target cloud is null or empty!" << std::endl;
        return false;
    }
    std::lock_guard<std::mutex> lock(mutex_);
    target_ = target;
    ndt_.setInputTarget(target_);
    return true;
}

// ScanMatch实现（与头文件纯虚函数签名完全一致：参数类型、顺序、返回值）
bool PCLNDT::ScanMatch(const CloudPtr& source,
                       const Eigen::Matrix4f& predict_pose,
                       CloudPtr& transformed_source,
                       Eigen::Matrix4f& result_pose) {
    // 输入有效性检查（避免空指针）
    if (!source || source->empty() || !target_ || target_->empty()) {
        std::cerr << "[PCLNDT] ScanMatch failed: Source or target cloud is invalid!" << std::endl;
        last_fitness_ = std::numeric_limits<double>::infinity();
        return false;
    }

    // 同步NDT参数（确保使用最新配置）
    ndt_.setResolution(resolution_);
    ndt_.setMaximumIterations(max_iter_);
    ndt_.setTransformationEpsilon(trans_eps_);

    // 设置输入源点云
    ndt_.setInputSource(source);

    // 执行NDT配准
    pcl::PointCloud<PointT> aligned_cloud;
    ndt_.align(aligned_cloud, predict_pose);

    // 检查配准是否收敛
    if (!ndt_.hasConverged()) {
        std::cerr << "[PCLNDT] ScanMatch warning: NDT did not converge!" << std::endl;
        last_fitness_ = std::numeric_limits<double>::infinity();
        return false;
    }

    // 提取配准结果
    last_fitness_ = ndt_.getFitnessScore();
    result_pose = ndt_.getFinalTransformation();  // NDT的最终变换已包含初始位姿

    // 设置输出点云（使用boost智能指针）
    transformed_source = boost::make_shared<pcl::PointCloud<PointT>>(aligned_cloud);

    return true;
}

// GetFitnessScore实现（const修饰，与头文件一致）
double PCLNDT::GetFitnessScore() const {
    return last_fitness_;
}

} // namespace lio_ndt
